/*
    robot main
*/


#include "robolite/robolite.h"   //include RoboLite kit

void mazeLoop(void)
{
    while(1)
    {
        if(!impact(RL_SENSOR_FORE)
                &&impact(RL_SENSOR_LEFT))
        {
            //
            rl_forward();
        }

        if(impact(RL_SENSOR_FORE)&&!impact(RL_SENSOR_LEFT))
        {
            rl_turnRight();
            rl_delay_ms(100);
            rl_turnRight_fast();
            rl_delay_ms(50);
            rl_stop();
            
        }
        
        if(impact(RL_SENSOR_LEFT)&&impact(RL_SENSOR_FIRE)){
                rl_turnLeft();
                rl_delay_ms(50);
                rl_forward();
        
        }

        if(impact(RL_SENSOR_LEFT)
                &&impact(RL_SENSOR_FORE))
        {
            if(impact(RL_SENSOR_FIRE))
            {
                rl_turnLeft();
                rl_delay_ms(50);
                rl_forward();
                
            }
            else
            {
                rl_backward();

                rl_turnRight_fast();

            }


        }


        if(!impact(RL_SENSOR_FORE)
                &&!impact(RL_SENSOR_LEFT))
        {
            rl_turnLeft_fast();
            rl_delay_ms(80);
            rl_turnLeft();
            rl_delay_ms(200);

        }


        /*
                if(impact(RL_SENSOR_BOTTOM)
                   &&impact(RL_SENSOR_FORE)
                   &&impact(RL_SENSOR_LEFT))
                {
                    //white line detected
                    rl_forward();
                    delay_ms(500);
                    rl_stop();

                    delay_ms(500);
                }
        */
        rl_detectSensors();
    }

}



void main(void)
{

    rl_init_devices();
    //rl_waitSound(3);

    //rl_motor(130,254);
    //rl_delay_s(1);
    
    mazeLoop();

    //rl_motor_ex(-130,255);
    //while(1);

}
